#include "stm32f10x.h"                  // Device header
#include "CarPWM.h"
#include "PID.h"
#include "OLED.h"
#include <stdio.h>
#include <stdlib.h>

int Motor1_PID_Out,Motor2_PID_Out;
extern tPid PidMotor1;
extern tPid PidMotor2;
extern int speed1,speed2;


void Car_Move(int PL,int PR)
{
	if(PL==0){PWM_D0(0);PWM_D1(0);}
	if(PL>0){PWM_D0(PL);PWM_D1(0);}
	if(PL<0){PWM_D0(0);PWM_D1(-PL);}
	
	if(PR==0){PWM_D2(0);PWM_D3(0);}
	if(PR>0){PWM_D2(PR);PWM_D3(0);}
	if(PR<0){PWM_D2(0);PWM_D3(-PR);}
}

void Car_MovePid(void)
{
	Motor1_PID_Out=PID_realize(&PidMotor1,speed1);
	if(Motor1_PID_Out>100)Motor1_PID_Out=100;
	if(Motor1_PID_Out<-100)Motor1_PID_Out=-100;
	
	Motor2_PID_Out=PID_realize(&PidMotor2,speed2);
	if(Motor2_PID_Out>100)Motor2_PID_Out=100;
	if(Motor2_PID_Out<-100)Motor2_PID_Out=-100;
	
	Car_Move(Motor1_PID_Out+1,Motor2_PID_Out);
}

void Car_SetSpeed(float Motor1SetSpeed,float Motor2SetSpeed)
{
	PidMotor1.target_val=Motor1SetSpeed;
	PidMotor2.target_val=Motor2SetSpeed;
}
